/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
* Test program for Inertia Link driver (only linux)
* 
* $Id: rt-tester.cpp,v 1.2 2009-08-27 12:41:06 jari Exp $
* $Date: 2009-08-27 12:41:06 $
* $Author: jari $
*/

#include "CInertiaLinkDriver.h"
#include <math.h>
#include<stdio.h>
#include <stdlib.h>
//#include<ostream>
//#include<iostream>
#include "newmat.h"
//#include "newmatio.h" 

using namespace NEWMAT; 

static float LONG = 60 * M_PI /180;     ///Kyseisen paika longitudi
static double timer_fac = 19660800;
static double timer_round_fac = pow(2,32)/timer_fac;

int factorial(int i) {
		if (i <= 1)
				return i;
		return (i * factorial(i - 1));
}

ColumnVector transformAccelerations(float a, float b, float c, float d, float ax, float ay, float az){
		Matrix C(3,3);           ///< Rotation matrix
		ColumnVector meas(3);    ///< measurement
		ColumnVector wacc(3);    ///< world frame

		
		C << 1-2*(c*c+d*d) << 2*(b*c-a*d)   << 2*(b*d+a*c)
		  << 2*(b*c+a*d)   << 1-2*(b*b+d*d) << 2*(c*d-a*b)
			<< 2*(b*d-a*c)   << 2*(c*d+a*b)   << 1-2*(b*b+c*c);
		
		meas(1) = ax; meas(2)=ay; meas(3)=az;
		
		wacc = C * meas;
		return wacc;
}

ColumnVector inverseTransformAccelerations(float a, float b, float c, float d, float ax, float ay, float az){
		Matrix C(3,3);           ///< Rotation matrix
		ColumnVector meas(3);    ///< measurement
		ColumnVector wacc(3);    ///< world frame

		
		C << 1-2*(c*c+d*d) << 2*(b*c-a*d)   << 2*(b*d+a*c)
						<< 2*(b*c+a*d)   << 1-2*(b*b+d*d) << 2*(c*d-a*b)
						<< 2*(b*d-a*c)   << 2*(c*d+a*b)   << 1-2*(b*b+c*c);
		
		meas(1) = ax; meas(2)=ay; meas(3)=az;
		
		wacc = (C.t()) * meas;
		return wacc;
}


int main(void){
		CInertiaLinkDriver IMU("/dev/ttyUSB0");
		FILE *f = fopen("IMU.txt","wt");
		int num = 0;
		unsigned int t;
		unsigned int t_old=0;
		double dt=0;
		float dataptr[200];
		
		float gee = 9.780318*(1+0.0053024*(sin(LONG)*sin(LONG))-0.0000058*(sin(2*LONG)*sin(2*LONG))); //Korjattu Gravitaatio
		
		usleep(1000*1000);
		IMU.stopContinousMode();
		usleep(1000*100);
		fprintf(stderr,"Calibrating the Gyro Bias\n");
		if(!IMU.captureGyroBias(1000)){
				fprintf(stderr,"FAILED!!\n");
		}
	
		fprintf(stderr,"Now reading continous meaurements type 0xC8 - using thread for parsing\n");
		usleep(1000*1000);
		if(!IMU.setContinousMode(0xC8,true)){
				fprintf(stderr,"FAILED!! - Exit!\n");
				exit(1);
		}
		
		///Variables for attitude
		float a,b,c,d; ///Quaternions
		float init_pitch,init_roll; ///Initial estimates
		int cnt = 0; ///loop counter
		float r,p,y; ///roll,pitch and yaw
		float dx,dy,dz; ///Delta values for angles (integrated within one timestep)
		float vx=0,vy=0,vz=0;
		float px=0,py=0,pz=0;
		float abx=0,aby=0,abz=0; ///Acceleration bias terms
		float my_yaw=0;
		int zcnt=0;
		//IMU.enableLog(f); ///< Sets internal file logging
		while(1){
				usleep(5*1000);
				
				if(IMU.getData(dataptr,t,num)){
						/*
						fprintf(stderr,"Read %d values from the IMU:",num);
						for(int i=0;i<3;i++){
								fprintf(stderr,"%.2f ",dataptr[i]);
						}
						fprintf(stderr,"\n");
						*/
						///Roll and pitch from the module itself
						float roll = atan2(dataptr[11],dataptr[14]);
						float pitch = asin(-dataptr[8]);
						float yaw = atan(dataptr[7]/dataptr[6]);//atan2(dataptr[6],dataptr[7]);
						///Initialization of quaternions
						if(cnt == 0){
								t_old = t;
								///Initial estimates calculated from accelerometer data
								init_pitch = -asin(-dataptr[0]);
								init_roll = -asin(dataptr[1]/cos(init_pitch));
								
								r=init_roll;
								p=init_pitch;
								y=0;
								
								///Quaternion representation
								a = cos(r/2.0)*cos(p/2.0)*cos(y/2.0)+sin(r/2.0)*sin(p/2.0)*sin(y/2.0);
								b = sin(r/2.0)*cos(p/2.0)*cos(y/2.0)-cos(r/2.0)*sin(p/2.0)*sin(y/2.0);
								c = cos(r/2.0)*sin(p/2.0)*cos(y/2.0)+sin(r/2.0)*cos(p/2.0)*sin(y/2.0);
								d = cos(r/2.0)*cos(p/2.0)*sin(y/2.0)+sin(r/2.0)*sin(p/2.0)*cos(y/2.0); ///plus eller minus?
								cnt++;
								
								continue;
						}
						///Time in seconds between samples (according to IMU-unit)
						dt = (t-t_old)/timer_fac;
						if(dt<0) dt+=timer_round_fac;
						
						dx = dataptr[3] * dt;
						dy = dataptr[4] * dt;
						dz = dataptr[5] * dt;
						my_yaw += dt*dataptr[5];
						float sigma = sqrt(dx*dx+dy*dy+dz*dz);
						
						float ac = cos(sigma/2.0); //;1-(0.5*sigma)*(0.5*sigma)/factorial(2) + pow(0.5*sigma,4)/factorial(4); //
						//float as= 0.5*(1- pow(0.5*sigma,2)/factorial(3)+pow(0.5*sigma,4)/factorial(5));
						float as;
						if(sigma < 0.0000001) as = 0.5;
						else as = sin(sigma/2.0)/sigma;
						
						if(zcnt<10){				
								///Quaternion for update
								float r0 = ac; 
								float r1 = as*dx;
								float r2 = as*dy;
								float r3 = as*dz;
								
								///Compute quaternion product t=q*r
								float t0 = r0*a-r1*b-r2*c-r3*d;
								float t1 = r0*b+r1*a-r2*d+r3*c;
								float t2 = r0*c+r1*d+r2*a-r3*b;
								float t3 = r0*d-r1*c+r2*b+r3*a;
								float norm = sqrt(t0*t0+t1*t1+t2*t2+t3*t3);
								a = t0/norm;
								b = t1/norm;
								c = t2/norm;
								d = t3/norm; 
						}
						float angvel = sigma/dt*180.0 /M_PI;
						
						///Zupt
						if(angvel<0.9){
							zcnt++;  //zcnt=0;
							if(zcnt>10){
									init_pitch = -asin(-(dataptr[0]));
									init_roll = -asin((dataptr[1])/cos(init_pitch));
									r=init_roll;
									p=init_pitch;
									///Quaternion representation
									a = cos(r/2.0)*cos(p/2.0)*cos(y/2.0)+sin(r/2.0)*sin(p/2.0)*sin(y/2.0);
									b = sin(r/2.0)*cos(p/2.0)*cos(y/2.0)-cos(r/2.0)*sin(p/2.0)*sin(y/2.0);
									c = cos(r/2.0)*sin(p/2.0)*cos(y/2.0)+sin(r/2.0)*cos(p/2.0)*sin(y/2.0);
									d = cos(r/2.0)*cos(p/2.0)*sin(y/2.0)+sin(r/2.0)*sin(p/2.0)*cos(y/2.0); ///plus eller minus?
									float normi = sqrt(a*a+b*b+c*c+d*d);
									a /=normi;
									b/=normi;
									c/=normi;
									d/=normi;			
									
									fprintf(stderr,"Z ");
									vx=0;vy=0;vz=0;
									ColumnVector wacc = transformAccelerations(a,b,c,d,dataptr[0],dataptr[1],dataptr[2]);
									wacc *=gee;
									wacc(3) += gee;
									abx = 0.9*abx+0.1*wacc(1);
									aby = 0.9*aby+0.1*wacc(2);
									abz = 0.9*abz+0.1*wacc(3); 
								
							}			
						}else{
								zcnt=0;
						}
						///Accelerations in world frame
						ColumnVector wacc = transformAccelerations(a,b,c,d,dataptr[0],dataptr[1],dataptr[2]);
						wacc *=gee;
						wacc(3) += gee-abz;
						//wacc(1) -= abx;
						//wacc(2) -= aby;
						
						vx += wacc(1)*dt;
						vy += wacc(2)*dt;
						vz += wacc(3)*dt;
						px += vx*dt;
						py += vy*dt;
						pz += vz*dt;
						
						
						
						///Back to eulers
						r = atan2(2*(a*b+c*d),1-2*(b*b+c*c));
						p = asin(2*(a*c-d*b));
						if(zcnt<=10) y = atan2(2*(a*d+b*c),1-2*(c*c+d*d));
						
						if(angvel>650) fprintf(stderr,"##################650!! \n");
						//fprintf(stderr,"DT=%.4f ",dt);
						//fprintf(stderr,"angSpeeds: W=%.2f (%.2f, %.2f,%.2f) ", 
						//																		angvel,dataptr[3]*180.0/M_PI,dataptr[4]*180.0/M_PI,dataptr[5]*180.0/M_PI);
						fprintf(stderr," RPY=(%.3f,%.3f,%.3f) rpy=(%.2f,%.2f %.2f) %.2f ",
																																		roll*180.0/M_PI, pitch*180.0/M_PI,yaw*180.0/M_PI,
																																		r*180.0/M_PI,p*180.0/M_PI,y*180.0/M_PI, my_yaw*180.0/M_PI);
						
						//fprintf(stderr,"acc_xyz=(%.2f,%.2f %.2f) ",wacc(1),wacc(2),wacc(3));
						fprintf(stderr,"xyz=(%.2f,%.2f %.2f) ", px,py,pz);

						fprintf(stderr,"\n");
						
						cnt++;
						t_old = t;				
				}else{
						//fprintf(stderr,"Failed to read IMU\n");
				}
				
				//bytes = IMU.Read(buf, 1);
				//fprintf(stderr,"GOT %d bytes\n",bytes);
				//for(int i=0;i<bytes;i++) fprintf(stderr,"0x%3X ", buf[i]);
				//fprintf(stderr,"\n");
				
		}
				
		
}

/**
* $Log: not supported by cvs2svn $
* Revision 1.1  2009-08-26 12:51:46  jari
* imu yritelmää
*
* Revision 1.9  2009-02-02 08:10:10  jari
* sisainen tallennus
*
* Revision 1.8  2009-02-01 12:12:46  jari
* nyt lukee dataa oikein
*
* Revision 1.7  2009-01-26 06:43:42  jari
* capture bias and then some
*
* Revision 1.6  2009-01-23 19:43:12  jari
* capture gyro bias
*
* Revision 1.5  2008-01-09 16:14:13  jari
* ilink dataa
*
* Revision 1.4  2007-12-11 11:15:49  jari
* Ajuri on valmis
*
* Revision 1.3  2007-12-09 22:09:03  jari
* Ajuri toimii
*
* Revision 1.2  2007-12-07 21:08:31  jari
* eteenpäin on menty
*
* Revision 1.1  2007-12-07 15:13:59  jari
* Inertia-Link driver
*
*/
